#pragma once

#include "rclcpp/rclcpp.hpp"
#include "io_hardinterface.h"
#include "can_speaker.h"
#include "io_sturct.h"
#include "common/common.h"

#include <agv_msgs/msg/read_in_puts.hpp>
#include <agv_msgs/msg/read_in_puts_new.hpp>
#include <agv_msgs/msg/output_port.hpp>
#include <agv_msgs/msg/agv_info.hpp>
#include <agv_msgs/msg/agv_auto_status.hpp>
#include <agv_msgs/msg/odometry_lite.hpp>
#include <agv_msgs/msg/out_puts_new.hpp>
#include <agv_msgs/msg/agv_event_status.hpp>
#include <agv_msgs/msg/agv_mode.hpp>
#include <agv_msgs/msg/read_in_puts_analog.hpp>
#include <agv_msgs/msg/outputs_analog.hpp>

#include <agv_srvs/srv/output_interface.hpp>
#include <agv_srvs/srv/outputs_new.hpp>
#include <agv_srvs/srv/outputs_analog.hpp>

#include <geometry_msgs/msg/pose_stamped.hpp>
#include <std_srvs/srv/trigger.hpp>
#include <chrono>
#include <thread>
#include <mutex>
#include <map>

enum SceneName
{
    SceneName0 = 0,
    SceneName1 = 1,
    SceneName2 = 2,
    SceneName3 = 3,
    SceneName4 = 4,
    SceneName5 = 5,
    SceneName6 = 6,
    SceneName7 = 7,
    SceneName8 = 8,
    SceneName9 = 9,
    SceneName10 = 10,
    SceneName11 = 11,
    SceneName12 = 12,
    SceneName13 = 13, // 特殊场景使用
    SceneName14 = 14, // for 北奔
    SceneName15 = 15, // 绿灯频闪-放行提示
    SceneName16 = 16  // 自旋时，提示自旋音量
};

namespace io_module {

class IoControl : public rclcpp::Node {
public:
    using DEVICE_TYPE = std::pair<std::string, int>;
    using MapOutputType = std::map<std::string, int>;
    using MapIterType = MapOutputType::iterator;
    using OUTPUT_IO_TYPE = agv_msgs::msg::PairTypeInt;
    using IO_GROUP = std::vector<OUTPUT_IO_TYPE>;

    explicit IoControl(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
    ~IoControl();

    bool init();
    bool update();

private:
    // ROS2 Interface
    rclcpp::Service<agv_srvs::srv::OutputInterface>::SharedPtr m_outputServer;
    rclcpp::Service<agv_srvs::srv::OutputsNew>::SharedPtr m_outputServerNew;
    rclcpp::Service<agv_srvs::srv::OutputsAnalog>::SharedPtr m_outputAServer;
    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr m_srv_green_blink;

    rclcpp::Publisher<agv_msgs::msg::ReadInPuts>::SharedPtr m_inputPub;
    rclcpp::Publisher<agv_msgs::msg::ReadInPutsNew>::SharedPtr m_inputNewPub;
    rclcpp::Publisher<agv_msgs::msg::ReadInPuts>::SharedPtr m_rawInputPub;
    rclcpp::Publisher<agv_msgs::msg::OutPutsNew>::SharedPtr m_outputNewPub;
    rclcpp::Publisher<agv_msgs::msg::ReadInPutsAnalog>::SharedPtr m_inputAPub;
    rclcpp::Publisher<agv_msgs::msg::OutputsAnalog>::SharedPtr m_outputAPub;

    rclcpp::Subscription<agv_msgs::msg::AGVMode>::SharedPtr m_agvinfoSub;
    rclcpp::Subscription<agv_msgs::msg::AGVEventStatus>::SharedPtr m_eventSub;
    rclcpp::Subscription<agv_msgs::msg::AgvAutoStatus>::SharedPtr m_autoRunSub;
    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr m_landMarkSub;
    rclcpp::Subscription<agv_msgs::msg::OdometryLite>::SharedPtr m_odomSub;
    rclcpp::Subscription<agv_msgs::msg::OutPutsNew>::SharedPtr m_setOutputSub;

    // Core Components
    std::shared_ptr<IoHardInterface> io_interface;
    
    // can喇叭
    std::shared_ptr<CanSpeaker> m_canSpeaker;

    // 强行设置绿灯闪烁
    std::shared_ptr<BlinkTrigger> m_blink;

    // 当前实时更新的二维码id
    std::shared_ptr<CDataTimeStamp<int>> m_curMarkId;

    // 当前的声音模式
    SoundMode m_soundMode;

    // 记录agvinfo的状态
    AgvStateInfo m_agvInfo;

    // Threading
    std::thread m_thread;
    std::timed_mutex m_quitMutex;
    
    // Data Members
    agv_msgs::msg::AGVEventStatus m_curEvent;
    agv_msgs::msg::AgvAutoStatus m_curAutoStatus;

    IoGroup m_ioGroup;

    // 灯光设置参数
    Lamp m_lamp;

    // 特殊场景使用
    // SpecialScene m_specialScene;

    // 左右转标志  0-左转  1-右转   2-灭
    int turn_flag;

    //
    // 输出io列表状态
    agv_msgs::msg::OutPutsNew m_outputsNewMsg;

    // 输出模拟量输出状态
    agv_msgs::msg::OutputsAnalog m_outputsAMsg;
    
    // 声光设备-输出设备
    // 名字：索引
    MapOutputType light_sound_devices = {
        {"greenLight", -1},
        {"yellowLight", -1},
        {"redLight", -1},
        {"speaker_1", -1}, // 运行
        {"speaker_2", -1},
        {"leftGreenLight", -1},
        {"rightGreenLight", -1},
        {"leftYellowLight", -1},
        {"rightYellowLight", -1},
        {"leftRedLight", -1},
        {"rightRedLight", -1},
        {"frontLeftGreenLight", -1},
        {"frontRightGreenLight", -1},
        {"frontLeftYellowLight", -1},
        {"frontRightYellowLight", -1},
        {"frontLeftRedLight", -1},
        {"frontRightRedLight", -1},
        {"rearLeftGreenLight", -1},
        {"rearRightGreenLight", -1},
        {"rearLeftYellowLight", -1},
        {"rearRightYellowLight", -1},
        {"rearLeftRedLight", -1},
        {"rearRightRedLight", -1}}; // 报警


    std::map<std::string, std::vector<DEVICE_TYPE>> new_input_devices;
    std::map<std::string, std::vector<DEVICE_TYPE>> new_output_devices;


    std::vector<DEVICE_TYPE> input_devices = {
        {"stopButton", -1},           // bit0  2023.9.25 第0个字段代表停止按钮
        {"resetButton", -1},          // bit1  复位按钮
        {"emergencyButton", -1},      // bit2  急停1按钮
        {"frontBumper", -1},          // bit3  前触边
        {"rearBumper", -1},           // bit4  后触边
        {"magDetect", -1},            // bit5  地标传感器
        {"turntablelowerLimit", -1},  // bit6  转盘低限位-废弃
        {"obstacleStop", -1},         // bit7  避障停止
        {"obstacleDeceleration", -1}, // bit8  避障减速
        {"obstacleAlert", -1},        // bit9  避障报警
        {"obstacleFailure", -1},      // bit10 避障错误
        {"rotationHome", -1},         // bit11 旋转零位-废弃
        {"emergencyButton2", -1},     // bit12 2023.4.19 第十二个字段代表急停2
        {"releaseButton", -1},        // bit13 2023.9.06 第十三个字段代表放行按钮
        {"upLimitFront", -1},         // bit14 2023.9.07 第14个字段代表前举升上限位
        {"downLimitFront", -1},       // bit15 2023.9.07 第15个字段代表前举升下限位
        {"upLimitRear", -1},          // bit16 2023.9.07 第16个字段代表后举升上限位
        {"downLimitRear", -1},        // bit17 2023.9.07 第17个字段代表后举升下限位
        {"leftBumper", -1},           // bit18 2023.9.27 第18个字段代表左触边
        {"rightBumper", -1},          // bit19 2023.9.27 第19个字段代表右触边
        {"brakeIn", -1},              // bit20 2023.9.27 第20个字段代表抱闸输入
        {"manulAutoBtn", -1},         // bit21 2023.9.27 第21个字段代表手自动旋钮
        {"chargeAccept", -1},         // bit22 2023.9.27 第22个字段代表充电接收端
        // 2023.12.16 add by haibo
        {"leftStop", -1},           // 23
        {"leftSlow", -1},           // 24
        {"leftWarning", -1},        // 25
        {"rightStop", -1},          // 26
        {"rightSlow", -1},          // 27
        {"rightWarning", -1},       // 28
        {"rearStop", -1},           // 29
        {"rearSlow", -1},           // 30
        {"leftForkTipSensor", -1},  // 31
        {"rightForkTipSensor", -1}, // 32
        {"leftLoadInplace", -1},    // 33
        {"rightLoadInplace", -1},   // 34
        // 2023.12.23 modify by haibo
        {"loadStatus", -1},     // 35
        {"driveManulAuto", -1}, // 36
        // 2023.12.23 modify by liabaocheng
        {"frontStop", -1},         // 37
        {"frontSlow", -1},         // 38
        {"frontWarning", -1},      // 39
        {"frontError", -1},        // 40
        {"rearWarning", -1},       // 41
        {"rearError", -1},         // 42
        {"leftError", -1},         // 43
        {"rightError", -1},        // 44
        {"leftFrontStop", -1},     // 45
        {"leftFrontSlow", -1},     // 46
        {"leftFrontWarning", -1},  // 47
        {"leftFrontError", -1},    // 48
        {"leftRearStop", -1},      // 49
        {"leftRearSlow", -1},      // 50
        {"leftRearWarning", -1},   // 51
        {"leftRearError", -1},     // 52
        {"rightFrontStop", -1},    // 53
        {"rightFrontSlow", -1},    // 54
        {"rightFrontWarning", -1}, // 55
        {"rightFrontError", -1},   // 56
        {"rightRearStop", -1},     // 57
        {"rightRearSlow", -1},     // 58
        {"rightRearWarning", -1},  // 59
        {"rightRearError", -1},    // 60

    };
    
    bool initParam();
    void rosInit();
    void readParam();
    void readPolarityParam();
    void readAnalogParam();
    void readInOutParam();
    void initIODevice(std::map<std::string, std::vector<DEVICE_TYPE>>& devices, const std::string& param_path);
    void initOutputsNewMsg();
    void initOutputsAMsg();
    void pubReadInputs();
    void pubReadInputsA();
    void recordOutputs(IO_GROUP outputs);
    void recordOutputsA(std::vector<agv_msgs::msg::PairTypeAnalog> outsA);
    void pubOutPutsNew();
    void pubOutPutsA();
    void threadFun();

    // Callbacks
    void agvinfoCallBack(const agv_msgs::msg::AGVMode::SharedPtr& msg);
    void eventCallBack(const agv_msgs::msg::AGVEventStatus::SharedPtr& event);
    void odomCallBack(const agv_msgs::msg::OdometryLite::SharedPtr& msg);
    void autoRunStatusCallBack(const agv_msgs::msg::AgvAutoStatus::SharedPtr& status);
    void getLandMarkCB(const geometry_msgs::msg::PoseStamped::SharedPtr& mark_data);
    void setOutputCallback(const agv_msgs::msg::OutPutsNew::SharedPtr& msg);

    // Service Handlers
    bool outputSrvCallBack(const std::shared_ptr<agv_srvs::srv::OutputInterface::Request> req,
                         std::shared_ptr<agv_srvs::srv::OutputInterface::Response> resp);
    bool outputSrvNewCallBack(const std::shared_ptr<agv_srvs::srv::OutputsNew::Request> req,
                            std::shared_ptr<agv_srvs::srv::OutputsNew::Response> resp);
    bool outputASrvCallBack(const std::shared_ptr<agv_srvs::srv::OutputsAnalog::Request> req,
                           std::shared_ptr<agv_srvs::srv::OutputsAnalog::Response> resp);
    bool handleGreenBlink(const std::shared_ptr<std_srvs::srv::Trigger::Request> req,
                        std::shared_ptr<std_srvs::srv::Trigger::Response> resp);

    // Business Logic
    bool setOutputs(std::vector<agv_msgs::msg::OutputPort> outports);
    bool setOutputsA(std::vector<agv_msgs::msg::PairTypeAnalog> outsA);
    void setRgyLamp(bool green, bool yellow, bool red, bool isblink, int on, int off);
    void light();
    void sound();
    void updateSoundLightByScene();
    void specialIoOut();
    bool backward();
    bool navRun();
    bool opRun();
    bool spinRun();
    int getNavType();
    bool findQr();
    void ioSpeaker();
    bool ioExist(std::string key);
    bool soundLightIoSet(std::string mode, const MapIterType &ioIter);
};

}